#include "Prm.h"
#include "Library.hpp"

#include <Windows.h>

const double Prm::LOCAL_PLANNER_STEP_SIZE=1.0;

Prm::Prm (	int number_vertices,
			int k_nearest,
			CollisionDetector* col,
			MultiRobotsSampler* sampler,
			ConfSet start,
			ConfSet target,bool use_distance) :
	m_number_vertices(number_vertices),
	m_k_nearest(k_nearest), 
	m_col(col),
	m_sampler(sampler),
	m_start(start),
	m_target(target),
	_use_distance(use_distance)
{
	m_graph = new Graph<int>(0,true);
	m_kd_tree = new Kd_tree_d<Kernel_d>();
	_null_prm=false;
	_clearance_detector_r1_pointer=NULL;
	_clearance_detector_r2_pointer=NULL;
}

Prm::Prm()
{
	_null_prm=true;
}

bool Prm::is_null()
{
	return _null_prm;
}

Prm::~Prm()
{
/*	delete m_graph;
	delete m_kd_tree; */
}

bool Prm::has_path()
{
	if(_path.size()==0)
		return false;
	return true;
}

void Prm::generate_roadmap()
{
	int number_to_add=m_number_vertices;
	m_number_vertices=0;
	add_vertices(number_to_add);
	add_start_and_target();
}

void Prm::add_vertices(UINT32 number_vertices)
{
	UINT32 baseline_vertex_index=m_all_tree_points.size();
	vector<Point_d> new_points;
	int i=0;
	for (; i < number_vertices; ++i)
	{
		ConfSet configurations_set = m_sampler->generate_sample_no_obstacles();
		Point_d configuration_point(get_point_d(configurations_set));

		m_kd_tree->insert(configuration_point);
		m_all_tree_points.push_back(configuration_point);
		new_points.push_back((configuration_point));
		m_graph->add_vertex(i+baseline_vertex_index);
	}

	i = 0;
	printf("There are %d points, adding: %d\n",m_number_vertices,number_vertices);
	m_number_vertices+=number_vertices;
	for (vector<Point_d>::iterator current_point_it = new_points.begin();
		current_point_it != new_points.end();
		current_point_it++)
	{
		add_point_to_graph(*current_point_it, (i++)+baseline_vertex_index);
	}
	printf("add points, there are now:%d\n",m_number_vertices);
}

Point_d Prm::get_point_d(ConfSet c) const
{
	double temp_array[] =  {CGAL::to_double(c[0].x()),
							CGAL::to_double(c[0].y()),
							CGAL::to_double(c[1].x()),
							CGAL::to_double(c[1].y())};

	Point_d configuration_point(4, temp_array, temp_array + 4);
	return configuration_point;
}

void Prm::add_connections(const std::vector<std::vector<ConfSet>>& points_to_add)
{
	printf("begin interconnecting\n");
	for(UINT32 i=0;i<points_to_add.size();i++)
	{
		
		if(m_col->local_planner(points_to_add[i][0], points_to_add[i][1],LOCAL_PLANNER_STEP_SIZE))
		{
			UINT32 start_point_index = get_point_index(get_point_d(points_to_add[i][0]));
			UINT32 end_point_index = get_point_index(get_point_d(points_to_add[i][1]));
			m_graph->add_edge(start_point_index, end_point_index,calculate_edge_weight(points_to_add[i][0],points_to_add[i][1]));
			printf("Connecting %s and %s\n",Library::conf_to_string(points_to_add[i][0]).c_str(),Library::conf_to_string(points_to_add[i][1]).c_str());
		}
		
	}
	printf("end interconnecting\n");
}

void Prm::add_path(const std::vector<ConfSet>& new_path)
{
	if(new_path.size()==0)
		return;
	ConfSet prev_configuration=new_path[0];
	printf("adding path\n");
	for(UINT32 i=1;i<new_path.size();i++)
	{
		ConfSet current_configuration=new_path[i];
		UINT32 new_index=get_point_index(get_point_d(current_configuration));
		if(new_index==0)
		{
			m_all_tree_points.push_back(get_point_d(current_configuration));
			m_kd_tree->insert(get_point_d(current_configuration));
			new_index=m_all_tree_points.size()-1;
			m_graph->add_vertex(new_index);
		}
		UINT32 prev_point_index=get_point_index(get_point_d(prev_configuration));
		m_graph->add_edge(prev_point_index, new_index,calculate_edge_weight(prev_configuration,current_configuration));
		printf("Connecting %s and %s\n",Library::conf_to_string(prev_configuration).c_str(),Library::conf_to_string(current_configuration).c_str());
		prev_configuration=current_configuration;
	}
	printf("done adding path\n");
}

void Prm::add_point_to_graph(Point_d p, int index)
{
	std::cout << index << "/" << m_number_vertices << std::endl;
	vector<Point_d> k_nearest_neighbors;

	m_kd_tree->k_nearest_neighbors(p, m_k_nearest, std::back_inserter(k_nearest_neighbors));

	ConfSet point_set = get_conf_set(p);

	for (vector<Point_d>::const_iterator k_nearest_neighbors_it = k_nearest_neighbors.begin();
		k_nearest_neighbors_it != k_nearest_neighbors.end();
		k_nearest_neighbors_it++)
	{
		int new_index = get_point_index(*k_nearest_neighbors_it);
		if(!m_graph->is_in_same_cc(index,new_index))
		{
			//ConfSet neighbor_point_set = get_conf_set(*k_nearest_neighbors_it);

			//cout << "b\n";
			if (m_col->local_planner(point_set, get_conf_set(*k_nearest_neighbors_it), LOCAL_PLANNER_STEP_SIZE))
			{
				/*cout << "Adding an edge between (" << point_set[0].x() << "," << point_set[0].y() << ") (" << point_set[1].x() << "," << point_set[1].y() << ")";
				cout << "and (" << neighbor_point_set[0].x() << "," << neighbor_point_set[0].y() << ") (" << neighbor_point_set[1].x() << "," << neighbor_point_set[1].y() << ")\n";
				cout << "From " << index << " to " << new_index << "\n";*/
				m_graph->add_edge(index, new_index,calculate_edge_weight(point_set,get_conf_set(*k_nearest_neighbors_it)));
			}
			//cout << "a\n";
		}
	}
}

int Prm::get_point_index(Point_d p)
{
	int i = 0;

	for (vector<Point_d>::iterator all_tree_points_it = m_all_tree_points.begin();
		all_tree_points_it != m_all_tree_points.end();
		all_tree_points_it++)
	{
		if (*all_tree_points_it == p)
		{
			return i;
		}

		i++;
	}

	return 0;
}

void Prm::add_start_and_target()
{
	_start_index = m_all_tree_points.size();

	Point_d start_configuration_point(get_point_d(m_start));
	m_all_tree_points.push_back(start_configuration_point);
	m_graph->add_vertex(_start_index);
	
	

	_end_index=_start_index+1;
	Point_d target_configuration_point(get_point_d(m_target));
	m_all_tree_points.push_back(target_configuration_point);
	m_graph->add_vertex(_end_index);
	
	m_number_vertices+=2;
}

void Prm::add_start_and_target_to_graph()
{
	Point_d start_configuration_point(get_point_d(m_start));
	Point_d target_configuration_point(get_point_d(m_target));
	add_point_to_graph(start_configuration_point, _start_index);
	add_point_to_graph(target_configuration_point, _end_index);
}

void Prm::generate_path()
{
	_path=vector<ConfSet>();
	std::cout << "retrieve path" << std::endl;
	std::list<int> temp_path;
	add_start_and_target_to_graph();
	_path_quality=m_graph->find_path(_start_index,_end_index,temp_path);
	if(temp_path.size()==0)
		return;
	for (std::list<int>::iterator path_it = temp_path.begin();
		path_it != temp_path.end();
		++path_it)
	{
		//cout << "add to path " << *path_it << "\n";
		_path.push_back(get_conf_set(m_all_tree_points[*path_it]));
	}
}

vector<ConfSet> Prm::retrieve_path()
{
	printf("Printing path:\n");
	for(UINT32 i=0;i<_path.size();i++)
	{
		printf("%s\n",Library::conf_to_string(_path[i]).c_str());
	}
	printf("end of path\n");
	return _path;
}

double Prm::get_path_quality()
{
	return _path_quality;
}

ConfSet Prm::get_conf_set(Point_d point)
{
	//cout << "get config\n";
	ConfSet configuration;

	Point_2 robot_1_conf(
		CGAL::to_double(point[0]),
		CGAL::to_double(point[1]));

	configuration.push_back(robot_1_conf);

	Point_2 robot_2_conf(
		CGAL::to_double(point[2]),
		CGAL::to_double(point[3]));

	configuration.push_back(robot_2_conf);

	//cout << "config (" << robot_1_conf.x() << "," << robot_1_conf.y() << ") (" << robot_2_conf.x() << "," << robot_2_conf.y() << ")\n";

	return configuration;
}

void Prm::set_clearance_detector(Clearance_detector_ie* detector1,Clearance_detector_ie* detector2,double alpha)
{
	_clearance_detector_r1_pointer=detector1;
	_clearance_detector_r2_pointer=detector2;
	_alpha=alpha;
}

double Prm::calculate_edge_weight(ConfSet p,ConfSet q)
{
	if(_use_distance)
	{
		return Library::configuration_distance(p,q);
	}
	if(_clearance_detector_r1_pointer==NULL || _clearance_detector_r2_pointer==NULL)
	{
		throw 1;
	}
	double step_size=LOCAL_PLANNER_STEP_SIZE;
	double accumilated_weight=0;
	Point_2 robot_1_start = p[0];
	Point_2 robot_2_start = p[1];
	Point_2 robot_1_target = q[0];
	Point_2 robot_2_target = q[1];

	double robot_1_x1 = CGAL::to_double(robot_1_start.x());
	double robot_1_y1 = CGAL::to_double(robot_1_start.y());
	double robot_1_x2 = CGAL::to_double(robot_1_target.x());
	double robot_1_y2 = CGAL::to_double(robot_1_target.y());

	double robot_1_distance = sqrt(pow(robot_1_x1 - robot_1_x2, 2) + pow(robot_1_y1 - robot_1_y2,2));

	// calculate how many steps are required
	int robot_1_step_num = floor((robot_1_distance - step_size) / step_size);
	double robot_1_vx = robot_1_x2 - robot_1_x1;
	double robot_1_vy = robot_1_y2 - robot_1_y1;

	double robot_2_x1 = CGAL::to_double(robot_2_start.x());
	double robot_2_y1 = CGAL::to_double(robot_2_start.y());
	double robot_2_x2 = CGAL::to_double(robot_2_target.x());
	double robot_2_y2 = CGAL::to_double(robot_2_target.y());

	double robot_2_distance = sqrt(pow(robot_2_x1 - robot_2_x2, 2) + pow(robot_2_y1 - robot_2_y2,2));

	// calculate how many steps are required
	int robot_2_step_num = floor((robot_2_distance - step_size) / step_size);
	double robot_2_vx = robot_2_x2 - robot_2_x1;
	double robot_2_vy = robot_2_y2 - robot_2_y1;

	int stemp_num = (robot_2_step_num < robot_1_step_num) ? robot_1_step_num : robot_2_step_num;

	double robot_1_partial_offset = step_size / (robot_1_distance - step_size);
	double robot_2_partial_offset = step_size / (robot_2_distance - step_size);

	for (int i = 1; i <= stemp_num; i++)
	{
		//cout << "local_planner index - " << i << "/" << stemp_num << "\n";
		// generate a configuration for every step
		double robot_1_offset =  i * robot_1_partial_offset;
		double robot_1_currx = robot_1_x1 + robot_1_vx * robot_1_offset;
		double robot_1_curry = robot_1_y1 + robot_1_vy * robot_1_offset;

		Conf robot_1_current_conf(robot_1_currx, robot_1_curry);

		// generate a configuration for every step
		double robot_2_offset =  i * robot_2_partial_offset;
		double robot_2_currx = robot_2_x1 + robot_2_vx * robot_2_offset;
		double robot_2_curry = robot_2_y1 + robot_2_vy * robot_2_offset;

		Conf robot_2_current_conf(robot_2_currx, robot_2_curry);
		
		Point_2_ie first_robot(robot_1_currx,robot_1_curry);
		Point_2_ie second_robot(robot_2_currx,robot_2_curry);
		// If an intermidiate configuration is invalid, return false
		accumilated_weight+=robot_1_offset/pow(_clearance_detector_r1_pointer->clearance(first_robot),_alpha);
		accumilated_weight+=robot_2_offset/pow(_clearance_detector_r2_pointer->clearance(first_robot),_alpha);
	}
	return accumilated_weight;
}